import lejos.nxt.*;

public class Braitenberg  {

	private static double beta = 0.5;
	private static int n = 5000;
	
	private static boolean terminate;

	// MotorPort constants
	private static final int FORWARD = 1;
	private static final int STOP = 3;
	
	
	private class WorkerThread extends Thread
	{
		private int value, normalized;

		private int average;
		private MotorPort stimulant, inhibitant;
		private SensorPort sensor;
		private int head;
		private int[] samples;
		private int MAX_LIGHT = 300;
		private int MIN_LIGHT = 600;
		
		public WorkerThread(SensorPort sensor, MotorPort stimulant, MotorPort inhibitant) {
			this.sensor = sensor;
			this.stimulant = stimulant;
			this.inhibitant = inhibitant;
			samples = new int[n];
		}
		
		public void run()
		{
			while (!terminate) {
				value = sensor.readRawValue();
				
				//samples[head] = value;
				head = (head+1) % n;
				
				for(int i = 0; i < n; i++) {
					if(samples[i] == 0)
						break;
					if(samples[i] < MAX_LIGHT)
						MAX_LIGHT = samples[i];
					if(samples[i] > MIN_LIGHT)
						MIN_LIGHT = samples[i];
				}
				
				normalized = normalize(value);
				
				//average = (int) ((beta * normalized) + ((1-beta)*average));

				//if(normalized > average) {
					stimulant.controlMotor(normalized, FORWARD);
				//} else {
					//stimulant.controlMotor(0, STOP);
				//}
				
				try {
					Thread.sleep(10);
				} catch (InterruptedException e) { }
			}
		}
		
		private int normalize(int light) {
			int output = (100 - ((light - MAX_LIGHT) * 100) / (MIN_LIGHT - MAX_LIGHT));

			if(output < 0) 
				return 0;
			if(output > 100) 
				return 100;

			return output;
		}
		
		public int getValue() {
			return value;
		}

		public int getAverage() {
			return average;
		}
		
		public int getNormalized() {
			return normalized;
		}
	}
	
	public void veh_2a() {
		LightSensor l1 = new LightSensor(SensorPort.S1);
		LightSensor l2 = new LightSensor(SensorPort.S2);
		l1.setFloodlight(false);
		l2.setFloodlight(false);
		
		WorkerThread leftSensorThread = new WorkerThread(SensorPort.S1, MotorPort.C, null);
		WorkerThread rightSensorThread = new WorkerThread(SensorPort.S2, MotorPort.B, null);
		leftSensorThread.start();
		rightSensorThread.start();
		
		while(!Button.ESCAPE.isPressed()) {
			LCD.drawInt(leftSensorThread.getValue(), 1, 0);
			LCD.drawInt(rightSensorThread.getValue(), 1, 1);

			LCD.drawInt(leftSensorThread.getNormalized(), 1, 3);
			LCD.drawInt(rightSensorThread.getNormalized(), 1, 4);
		}
		terminate = true;
		try {
			leftSensorThread.join();
			rightSensorThread.join();
		} catch (InterruptedException e) { }
	}
	
}

